function f = cloth_num(array,Restlength,D,m,g)
% Parameters
Restlength = 0.8;
D = 30000;
m = 80;
g = 9.81;
% Hulptensoren
tensor = array2tensor(array);
N=length(tensor(:,1,1));
hordistance=tensor(:,1:end-1,:)-tensor(:,2:end,:);
vertdistance=tensor(1:end-1,:,:)-tensor(2:end,:,:);
diagdistance1=tensor(1:end-1,1:end-1,:)-tensor(2:end,2:end,:);
diagdistance2=tensor(1:end-1,2:end,:)-tensor(2:end,1:end-1,:);
hornorm=zeros(N,N-1);
vertnorm=zeros(N-1,N);
diagnorm1=zeros(N-1,N-1);
diagnorm2=zeros(N-1,N-1);
Ldiag=ones(N-1,N-1)*1.5*Restlength;
Lhor=ones(N,N-1)*Restlength;
Lvert=ones(N-1,N)*Restlength;

for i=1:N,
    for j=1:N-1,
        hulparray(1:3,1)=hordistance(i,j,:);
        hornorm(i,j)=norm(hulparray);
        hulparray(1:3,1)=vertdistance(j,i,:);
        vertnorm(j,i)=norm(hulparray);
    end
end
for i=1:N-1,
    for j=1:N-1,
        hulparray(1:3,1)=diagdistance1(i,j,:);
        diagnorm1(i,j)=norm(hulparray);
        hulparray(1:3,1)=diagdistance2(j,i,:);
        diagnorm2(i,j)=norm(hulparray);
    end
end
horenergy=(max(hornorm-Lhor,zeros(N,N-1))).^2*D;
vertenergy=(max(vertnorm-Lvert,zeros(N-1,N))).^2*D;
% diagenergy=(max(diagnorm1-Ldiag,zeros(N-1,N-1))).^2*D+(max(diagnorm2-Ldiag,zeros(N-1,N-1))).^2*D;
diagenergy=0;
f=sum(sum(horenergy))+sum(sum(vertenergy))+sum(sum(diagenergy))+sum(sum(m*g*tensor(:,:,3)));


end